# -*- coding: utf-8 -*-
"""
Created on Tue Aug 16 09:47:47 2022

@author: wulong
"""
import numpy as np
from casadi import *
from Basic_paras import *

# In[1] Real system
# States
def ode_ies_r(x, uc, uz, ud):
    dxdt = SX.zeros(Nx_r)
    
    If = x[0]
    Gh2 = x[1]
    pO2 = x[2]
    pH2O = x[3]
    pH2 = x[4]
    Pmtf = x[5]
    tabf = x[6]
    tabw = x[7]
    tabt = x[8]
    tc = x[9]
    tcs = x[10]
    tcwm = x[11]
    te = x[12]
    tes = x[13]
    tewm = x[14]
    vcap = x[15]
    C_soc = x[16]
    Iba = x[17]
    C_sot = x[18]
    C_stc = x[19]
    C_sth = x[20]
    tre = x[21]
    tbr = x[22]
    
    Gff = uc[0]
    Gfm = uc[1]
    Gab = uc[2]
    Nec = uc[3]
    Gec = uc[4]
    Gstu = uc[5]
    Pbar = uc[6]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    Sra = ud[1]
    Pd = ud[2]
    Qother = ud[3]
    
    # real ST input with binary
    Gst = zst*Gstu + (zst - 1)*Gstu # ST
    mstc = C_sot*Mst
    
    # FC
    Ifc = fu_d/(2*Kr)*Gh2
    Gff_mol = Gff*Mch4
    Go2 = 1/r_HO*Gh2
    Gh2r = 2*Kr*If
    
    # ST in the return side
    msth = Mst - mstc
    tsth = C_sth/(msth + eps)
    thp = zst*tre + (1 - zst)*tsth
    
    # PIP in the rerurn side
    Gsl = Gab + Gec + Gst
    trec = (Gsl*tre - Gst*thp)/(Gab + Gec + eps)
    
    # MA
    tab = zma*(tab0 + tabf + tabw + tabt)
    
    # EC
    pc = exp(21.3-2025.5/(248.94+tc))/1e6
    pe = exp(21.3-2025.5/(248.94+te))/1e6
    hcro = -137.26+1.23463*(273.15+tc) - hspc
    hero = 338.02893+0.24532*(273.15+te) + hsph
    tcwo = 2*tcwm - tcwi
    tec = zec*(2*tewm - trec)
    pr = pc/pe # Compressor
    etavl = 0.98-0.085*(pr**(1/kr)-1)
    Gr = etavl*Nec*roeg*Vcp
    wi = kr/(1e3*(kr-1))*pe*1e6/roeg*(pr**((kr-1)/kr)-1)
    hcri = hero + wi
    heri = hcro # Expansion valve
    
    # PIP in the supply side 1
    tslc = (Gab*tab + Gec*tec)/(Gab + Gec + eps)
    
    # ST in the supply side
    tstc = C_stc/(mstc + eps)
    tcp = zst*(tstc) + (1 - zst)*tslc
    
    # PIP in the supply side 2
    tsl = (Gab*tab + Gec*tec + Gst*tcp)/(Gsl + eps)
    
    # BR
    Q_the = Qsl0*(tbr - tsl)/delta_twa0*(Gsl/Gsl0)**0.6
    tre_cal = tsl + Q_the/(Gsl*Cw)
    Qsl = Gsl*Cw*(tre - tsl)
    
    # BA
    iba = Iba/npb
    Vba = nsb*(Em - vcap - R0b*iba)
    Iba_cal = Pbar*1000/Vba
    
    dxdt[0] = zfc*((Ifc - If)/tau_e) # FC
    dxdt[1] = zfc*((Gff_mol - Gh2)/tau_f)
    dxdt[2] = zfc*((1/KO2*(Go2 - If*Kr) - pO2)/tau_O2)
    dxdt[3] = zfc*((1/KH2O*Gh2r - pH2O)/tau_H2O)
    dxdt[4] = zfc*((1/KH2*(Gh2- Gh2r) - pH2)/tau_H2)
    dxdt[5] = zma*((kma1*Gfm**2 + kma2*Gfm + kma3 - Pmtf)/tau_mtf) # MA
    dxdt[6] = zma*((kma4*Pmtf - tabf)/tau_abf)
    dxdt[7] = zma*((kma5*Gab + kma6 - tabw)/tau_abw)
    dxdt[8] = zma*((kma7*trec + kma8 - tabt)/tau_abt)
    dxdt[9] = zec*((Gr*(hcri - hcro) + alfar*Aci*(tcs - tc))/(Ccr*Mcr)) # EC
    dxdt[10] = zec*((alfar*Aci*(tc - tcs) + alfaw*Aco*(tcwm - tcs))/(Cs*Mcs))
    dxdt[11] = zec*((Cw*Gcw*(tcwi - tcwo) + alfaw*Aco*(tcs - tcwm))/(Cw*Mcw))
    dxdt[12] = zec*((Gr*(heri - hero) + alfar*Aei*(tes - te))/(Cer*Mer))
    dxdt[13] = zec*((alfar*Aei*(te - tes) + alfaw*Aeo*(tewm - tes))/(Cs*Mes))
    dxdt[14] = zec*((Cw*Gec*(trec - tec) + alfaw*Aeo*(tes - tewm))/(Cw*Mew))
    dxdt[15] = iba/C1b - vcap/(R1b*C1b) # BA
    dxdt[16] = -iba/(3600*Ceb)
    dxdt[17] = (Iba_cal - Iba)/tau_dc
    dxdt[18] = -Gst/Mst # ST
    dxdt[19] = -Gst*tcp
    dxdt[20] = Gst*thp
    dxdt[21] = (tre_cal - tre)/tau_ahu # BR
    dxdt[22] = (Ubr*(ta - tbr) - Qsl + Qother)/Cbr
    
    return dxdt

# Outputs 
def out_ies_r(x, uc, uz, ud):
    y_alg = DM.zeros(Ny_r)
    
    If = x[0]
    Gh2 = x[1]
    pO2 = x[2]
    pH2O = x[3]
    pH2 = x[4]
    Pmtf = x[5]
    tabf = x[6]
    tabw = x[7]
    tabt = x[8]
    tc = x[9]
    tcs = x[10]
    tcwm = x[11]
    te = x[12]
    tes = x[13]
    tewm = x[14]
    vcap = x[15]
    C_soc = x[16]
    Iba = x[17]
    C_sot = x[18]
    C_stc = x[19]
    C_sth = x[20]
    tre = x[21]
    tbr = x[22]
    
    Gff = uc[0]
    Gfm = uc[1]
    Gab = uc[2]
    Nec = uc[3]
    Gec = uc[4]
    Gstu = uc[5]
    Pbar = uc[6]
    
    zfc = uz[0]
    zma = uz[1]
    zec = uz[2]
    zst = uz[3]
    
    ta = ud[0]
    Sra = ud[1]
    Pd = ud[2]
    Qother = ud[3]
    
    # PV
    I_pv_max = I_max_0*Sra/Sra_pv_0*(1 + c_pv1*(ta - T_pv_0))
    V_pv_max = V_max_0*log(exp(1) + c_pv2*(Sra - Sra_pv_0))*(1 - c_pv3*(ta - T_pv_0))
    Ppv = npp*nsp*I_pv_max*V_pv_max/1000
        
    # FC
    eta_a = afc + bfc*log(If + eps)
    eta_c = -R0fc*T0fc/(2*F0)*log(1-If/IL + eps)
    eta_o = If*rfc
    V0 = N0fc*(E0fc + R0fc*T0fc/(2*F0)*log(pH2*sqrt(pO2)/(pH2O + eps) + eps))
    Vfc = V0 - eta_a - eta_c - eta_o
    Ifc = fu_d/(2*Kr)*Gh2
    Pfc = zfc*(Vfc*Ifc/1000)
    
    # PIP in the rerurn side
    Gall = Gab + Gec + Gstu
        
    # MA
    Pmt = zma*(Pmt0 + Pmtf)
    
    # EC
    pc = exp(21.3-2025.5/(248.94+tc))/1e6
    pe = exp(21.3-2025.5/(248.94+te))/1e6
    pr = pc/pe # Compressor
    etavl = 0.98-0.085*(pr**(1/kr)-1)
    etacp = 0.9085*exp(-0.06443*pr)-7.605*exp(-3.155*pr)
    Gr = etavl*Nec*roeg*Vcp
    wi = kr/(1e3*(kr-1))*pe*1e6/roeg*(pr**((kr-1)/kr)-1)
    Pcp = zec*(Gr*wi/etacp)
    
    # BR
    Hpmp = Spmp*Gall**2/(rhow*ge)
    eta_pmp = b0p*Gall**2 + b1p*Gall + b2p
    Ppmp = Gall*ge*Hpmp/(1e3*eta_pmp)
    
    # BA
    iba = Iba/npb
    Vba = nsb*(Em - vcap - R0b*iba)
    Pba = Vba*Iba/1000
    
    # Electric Power
    Psc = Pcp + Ppmp
    Psl = Ppv + Pfc + Pmt + Pba - Psc - Pd
    
    y_alg[0] = Psl
    y_alg[1] = tbr
    
    return y_alg

# In[2] Formulate discrete time dynamics
x_sym_r = SX.sym('x_r', Nx_r)
uc_sym_r = SX.sym('uc_r', Nuc_r)
uz_sym_r = SX.sym('uz_r', Nuz_r)
ud_sym_r = SX.sym('ud_r', Nud_r)
ode_sym_r = ode_ies_r(x_sym_r, uc_sym_r, uz_sym_r, ud_sym_r)
args_r = {'x': x_sym_r, 'p': vertcat(uc_sym_r, uz_sym_r, ud_sym_r), 'ode': ode_sym_r}
opts_r = {'tf': Delta_r, 'regularity_check': True}
I_ode_r = integrator('I_ode_r', 'rk', args_r, opts_r)

